/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.usfirst.frc;
import edu.wpi.first.wpilibj.*;
/**
 * Main loop
 * @author James
 */
public class FRCBot extends SimpleRobot {
    private boolean debug = false;
    private static int current_fiber = 0, debugState = 0;
    private void init(){ //Initialization
        Watchdog.getInstance().feed(); //Feed the watchdog
        Robot.instance = this; //Store internal data
    }
    public void autonomous(){ //Autonomous, incomplete
        //Line Following Program
        for(;;){ //Repeat until returned
            if(!isAutonomous()) //If no longer autonomous
                return; //End method
        }
    }
    public void lineFollow(){ //Line following program
        final double speed = 0.5; //Speed of the motors when moving
        int state = 100*lineCheck()+10*lineCheck()+lineCheck(); // Turn each check into a easy switchable integer
        switch(state){ //Decide upon the integer
            case 0: //Nothing, Stop
                Robot.fr.set(0);
                Robot.br.set(0);
                Robot.fl.set(0);
                Robot.bl.set(0);
                break;
            case 1: case 11: //Line on the right side, turn left
                Robot.fr.set(speed);
                Robot.br.set(speed);
                Robot.fl.set(0);
                Robot.bl.set(0);
                break;
            case 10: case 111: case 101: //Line in hte center, and oddity cases, go straight ahead
                Robot.fr.set(speed);
                Robot.br.set(speed);
                Robot.fl.set(speed);
                Robot.bl.set(speed);
                break;
            case 100: case 110: //Line on the left side, turn right
                Robot.fr.set(0);
                Robot.br.set(0);
                Robot.fl.set(speed);
                Robot.bl.set(speed);
                break;
        }
    }
    public char lineCheck(){ //Hypothetical line checking method, supposedly requires a input of which sensor, and tests for the line.
        if(true)
            return 1;
        else
            return 0;
    }
    public void startCompetition() { //Main loop
        init(); //Initialize
        for(;;){ // Loop forever
            Watchdog.getInstance().feed(); //feed the watchdog
            if(!debug){ //Switiching from debug and normal execution mode
                try{
                    normalExecution(); //Normal Execution
                }catch(Throwable t){
                    //Ignore, completely normal
                }
            }else{
                try{
                    debugModeExecution(); //Debug execution
                }catch(Throwable t){
                    //Ignore, completely normal
                }
            }
            debugCheck(); //Checks for debug switching
        } //repeat endlessly
    }
    private void normalExecution() throws Throwable{ //Normal execution
        Watchdog.getInstance().feed(); // Feed the watchdog
        if(++current_fiber >= Robot.fibers.length) //Increment fiber number
            current_fiber = 0; // reset if over the line
        Robot.fibers[current_fiber].run(); //Run the normal execution sequence
    }
    private void debugModeExecution() throws Throwable{ //Debug execution
        Watchdog.getInstance().feed(); // Feed the watchdog
        if(++current_fiber >= Robot.fibers.length)//Increment fiber number
            current_fiber = 0;// reset if over the line
        Robot.fibers[current_fiber].debugRun(); // Run the debug execution sequence
    }
    private void debugCheck(){ // Checks for the debug mode change
        Watchdog.getInstance().feed(); // Feed the watchdog
        if(Robot.driveStick.getRawButton(1)){ //Button Pressed
            switch(debugState){ //Change the button state
                case 0: //Button originally unpressed, normal mode
                    debugState = 3; //Switch to debug mode, pressed
                    break;
                case 1: //Button originally pressed, normal mode
                    break; // Do nothing
                case 2: //Button originally unpressed, debug mode
                    debugState = 1; // Switch to normal mode, pressed
                    break;
                case 3: //Button originally pressed, debug mode
                    break; //Do nothing
            }
        } else { //Button released
            switch(debugState){ //Change the button state
                case 0: //button originally unpressed, normal mode
                    break; // do nothing
                case 1: //Button originally presed, normal mode
                    debugState = 0; //Switch to unpressed, normal mode
                    break;
                case 2: //button originally unpressed, debug mode
                    break; // Do nothing
                case 3: // Button originally pressed, debug mode
                    debugState = 2; //Switch to unpressed, debug mode
                    break;
            }
        }
        debug = (debugState -2 < 0); //Switch the debug state dependingon button state
    }
    public static void yield() throws Throwable{ //Returns back to main loop
        throw new Exception();
    }
}
